/**
 * @file offb_node.cpp
 * @brief offboard example node, written with mavros version 0.14.2, px4 flight
 * stack and tested in Gazebo SITL
 */
#include "offboard.hpp"

void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

void scan_tf_callback(const sensor_msgs::LaserScan& msg){
    scan_msg = msg;
}

void odometry_callback(const nav_msgs::Odometry &current_info)
{
    time_current_update = ros::Time::now().toSec();
    current_point.x= current_info.pose.pose.position.x;
    current_point.y= current_info.pose.pose.position.y;
    current_point.z= current_info.pose.pose.position.z;
    current_angle.x= current_info.pose.pose.orientation.x;
    current_angle.y= current_info.pose.pose.orientation.y;
    current_angle.z= current_info.pose.pose.orientation.z;
    current_angle.w= current_info.pose.pose.orientation.w;
    curr_angle = toEulerAngle(current_angle);
    geometry_msgs::Point rotation_hehe_data = rotation_hehe(curr_angle);
    current_point.x += rotation_hehe_data.x;
    current_point.y += rotation_hehe_data.y;
    current_point.z += rotation_hehe_data.z;

    current_yaw_angle = curr_angle.z;
    // current_update_flag = true;
}
 
void circlepoint_callback(const std_msgs::Float32MultiArray &circle_point)
{
    
	center_point.x = circle_point.data[0];
    center_point.y = circle_point.data[1];
    if(center_point.x>0 && center_point.y>0)
    {
        time_circle_update = ros::Time::now().toSec();
        circle_point_flag = true;
        // ROS_INFO("----->circle_point_global: %d %d", center_point.x, center_point.y);
    }

}


void gps_frommaster_callback(const sensor_msgs::NavSatFix &gps_msg)
{
    //subscribe gps odometry from other vehicle
    if(gps_msg.position_covariance[1]==1)
    {
        othervehicle_1.vehicleId = 1;
        othervehicle_1.latitude = gps_msg.latitude;
        othervehicle_1.longitude = gps_msg.longitude;
        othervehicle_1.altitude = gps_msg.altitude;
        othervehicle_1.command = gps_msg.position_covariance[0];
        time_vehicle1_pos_update = ros::Time::now().toSec();
        time_vehicle1_pos_flag = true;
    }
    else if(gps_msg.position_covariance[1]==2)
    {
        othervehicle_2.vehicleId = 2;
        othervehicle_2.latitude = gps_msg.latitude;
        othervehicle_2.longitude = gps_msg.longitude;
        othervehicle_2.altitude = gps_msg.altitude;
        othervehicle_2.command = gps_msg.position_covariance[0];
        time_vehicle2_pos_update = ros::Time::now().toSec();
        time_vehicle2_pos_flag = true;
    }
    else if(gps_msg.position_covariance[1]==3)
    {
        othervehicle_3.vehicleId = 3;
        othervehicle_3.latitude = gps_msg.latitude;
        othervehicle_3.longitude = gps_msg.longitude;
        othervehicle_3.altitude = gps_msg.altitude;
        othervehicle_3.command = gps_msg.position_covariance[0];
        time_vehicle3_pos_update = ros::Time::now().toSec();
        time_vehicle3_pos_flag = true;
    }
    else if(gps_msg.position_covariance[1]==4)
    {
        othervehicle_4.vehicleId = 4;
        othervehicle_4.latitude = gps_msg.latitude;
        othervehicle_4.longitude = gps_msg.longitude;
        othervehicle_4.altitude = gps_msg.altitude;
        othervehicle_4.command = gps_msg.position_covariance[0];
        time_vehicle4_pos_update = ros::Time::now().toSec();
        time_vehicle4_pos_flag = true;
    }



    //clear the out time data
    if(ros::Time::now().toSec() - time_vehicle1_pos_update > 1)
    {
        time_vehicle1_pos_flag = false;
    }
    if(ros::Time::now().toSec() - time_vehicle2_pos_update > 1)
    {
        time_vehicle2_pos_flag = false;
    }
    if(ros::Time::now().toSec() - time_vehicle3_pos_update > 1)
    {
        time_vehicle3_pos_flag = false;
    }
    if(ros::Time::now().toSec() - time_vehicle4_pos_update > 1)
    {
        time_vehicle4_pos_flag = false;
    }

}


void tagdetect_callback(const nav_msgs::Odometry &msg)
{
    int tag_level_temp = msg.pose.covariance[0];
    if(tag_level_temp==tag_size_lv1 && msg.pose.pose.position.z<8.0)
    {
        tag_point_lv1.x= msg.pose.pose.position.x;
        tag_point_lv1.y= msg.pose.pose.position.y;
        tag_point_lv1.z= msg.pose.pose.position.z;


        q_tag_lv1.x= msg.pose.pose.orientation.x;
        q_tag_lv1.y= msg.pose.pose.orientation.y;
        q_tag_lv1.z= msg.pose.pose.orientation.z;
        q_tag_lv1.w= msg.pose.pose.orientation.w;
        //防止没有位置信息导致角度乱转
        if(current_aid_angle.x==0&&current_aid_angle.y==0&&current_aid_angle.z==0&&current_aid_angle.w==0)
        {
            current_aid_angle.x=0;
            current_aid_angle.y=0;
            current_aid_angle.z=0;
            current_aid_angle.w=1;
        }
        q_vehicle = quatMultiply(q_cam_to_vehicle, q_tag_lv1);
        //qpt->qtp,inverse it
        q_vehicle.x = -q_vehicle.x;
        q_vehicle.y = -q_vehicle.y;
        q_vehicle.z = -q_vehicle.z;

        q_cam_to_vehicle_multi = quatMultiply(q_vehicle, current_aid_angle);
        //qtg
        q_cam_to_vehicle_inverse.x = q_cam_to_vehicle_multi.x;
        q_cam_to_vehicle_inverse.y = q_cam_to_vehicle_multi.y;
        q_cam_to_vehicle_inverse.z = q_cam_to_vehicle_multi.z;
        q_cam_to_vehicle_inverse.w = q_cam_to_vehicle_multi.w;
        // qpg  to rotation matrix
        Eigen::Matrix3d plane_odom_rotation= Quaternion2RotationMatrix(current_aid_angle.x, current_aid_angle.y, current_aid_angle.z, current_aid_angle.w);
        //qtg to rotation matrix
        Eigen::Matrix3d qgt_rotation= Quaternion2RotationMatrix(q_cam_to_vehicle_inverse.x, q_cam_to_vehicle_inverse.y, q_cam_to_vehicle_inverse.z, q_cam_to_vehicle_inverse.w);
        //Rpg multiply camera offset
        geometry_msgs::Vector3 rpgXcameraoffset =  R_multi_P(plane_odom_rotation, camera_offset_x, camera_offset_y, camera_offset_z);
        // tag_point_lv1.x set offset 0.2
        geometry_msgs::Vector3 rgtXpct =  R_multi_P(qgt_rotation, tag_point_lv1.x-0.2, tag_point_lv1.y, tag_point_lv1.z); 
        pose_tag_lv1.pose.position.x = current_aid_point.x + rpgXcameraoffset.x - rgtXpct.x;
        pose_tag_lv1.pose.position.y = current_aid_point.y + rpgXcameraoffset.y - rgtXpct.y;
        pose_tag_lv1.pose.position.z = current_aid_point.z + rpgXcameraoffset.z - rgtXpct.z;
        pose_tag_lv1.pose.orientation=q_cam_to_vehicle_multi;

        // low pass filter
        if(first_see_tag)
        {
            //init filter
            pose_tag_final_f = pose_tag_lv1;
            first_see_tag = false;
        }
        else{
            pose_tag_final_f = pose_tag_filter(pose_tag_lv1, pose_tag_final_f, 0.1);
        }
        

        //print yaw angle ,checking
        geometry_msgs::Vector3 temp11 = toEulerAngle(pose_tag_lv1.pose.orientation);
        // ROS_INFO("tag position 1:   %f %f %f",pose_tag_lv1.pose.position.x,pose_tag_lv1.pose.position.y,pose_tag_lv1.pose.position.z);
        // ROS_INFO("tag angle 1:   %f %f %f",temp11.x*rad2deg,temp11.y*rad2deg,temp11.z*rad2deg);
        time_tag_update_lv1 = ros::Time::now().toSec();
        tag_point_flag_lv1 = true;
        tag_point_any = true;
    }
    else if(tag_level_temp==tag_size_lv2 && msg.pose.pose.position.z<3.0)
    {
        tag_point_lv2.x= msg.pose.pose.position.x;
        tag_point_lv2.y= msg.pose.pose.position.y;
        tag_point_lv2.z= msg.pose.pose.position.z;
        q_tag_lv2.x= msg.pose.pose.orientation.x;
        q_tag_lv2.y= msg.pose.pose.orientation.y;
        q_tag_lv2.z= msg.pose.pose.orientation.z;
        q_tag_lv2.w= msg.pose.pose.orientation.w;
        if(current_aid_angle.x==0&&current_aid_angle.y==0&&current_aid_angle.z==0&&current_aid_angle.w==0)
        {
            current_aid_angle.x=0;
            current_aid_angle.y=0;
            current_aid_angle.z=0;
            current_aid_angle.w=1;
        }
        q_vehicle = quatMultiply(q_cam_to_vehicle, q_tag_lv2);
        //qpt->qtp,inverse it
        q_vehicle.x = -q_vehicle.x;
        q_vehicle.y = -q_vehicle.y;
        q_vehicle.z = -q_vehicle.z;

        q_cam_to_vehicle_multi = quatMultiply(q_vehicle, current_aid_angle);
        //qtg
        q_cam_to_vehicle_inverse.x = q_cam_to_vehicle_multi.x;
        q_cam_to_vehicle_inverse.y = q_cam_to_vehicle_multi.y;
        q_cam_to_vehicle_inverse.z = q_cam_to_vehicle_multi.z;
        q_cam_to_vehicle_inverse.w = q_cam_to_vehicle_multi.w;
        //get qpg 然后再转rotation
        Eigen::Matrix3d plane_odom_rotation= Quaternion2RotationMatrix(current_aid_angle.x, current_aid_angle.y, current_aid_angle.z, current_aid_angle.w);
        Eigen::Matrix3d qgt_rotation= Quaternion2RotationMatrix(q_cam_to_vehicle_inverse.x, q_cam_to_vehicle_inverse.y, q_cam_to_vehicle_inverse.z, q_cam_to_vehicle_inverse.w);

        geometry_msgs::Vector3 rpgXcameraoffset =  R_multi_P(plane_odom_rotation, camera_offset_x, camera_offset_y, camera_offset_z);
        // tag_point_lv2.x set offset 0.2
        geometry_msgs::Vector3 rgtXpct =  R_multi_P(qgt_rotation, tag_point_lv2.x-0.2, tag_point_lv2.y, tag_point_lv2.z); 
        pose_tag_lv2.pose.position.x = current_aid_point.x + rpgXcameraoffset.x - rgtXpct.x;
        pose_tag_lv2.pose.position.y = current_aid_point.y + rpgXcameraoffset.y - rgtXpct.y;
        pose_tag_lv2.pose.position.z = current_aid_point.z + rpgXcameraoffset.z - rgtXpct.z;
        pose_tag_lv2.pose.orientation=q_cam_to_vehicle_multi;

        
        // low pass filter
        if(first_see_tag)
        {
            pose_tag_final_f = pose_tag_lv2;
            first_see_tag = false;
        }
        else{
            pose_tag_final_f = pose_tag_filter(pose_tag_lv2, pose_tag_final_f, 0.1);
        }
        
        geometry_msgs::Vector3 temp11 = toEulerAngle(pose_tag_lv2.pose.orientation);
        // ROS_INFO("tag position:   %f %f %f",pose_tag_lv2.pose.position.x,pose_tag_lv2.pose.position.y,pose_tag_lv2.pose.position.z);
        // ROS_INFO("tag angle 2:   %f %f %f",temp11.x*rad2deg,temp11.y*rad2deg,temp11.z*rad2deg);
        time_tag_update_lv2 = ros::Time::now().toSec();
        tag_point_flag_lv2 = true;
        tag_point_any = true;
    }
    else if(tag_level_temp==tag_size_lv3 && msg.pose.pose.position.z<0.6)
    {
        tag_point_lv3.x= msg.pose.pose.position.x;
        tag_point_lv3.y= msg.pose.pose.position.y;
        tag_point_lv3.z= msg.pose.pose.position.z;
        q_tag_lv3.x= msg.pose.pose.orientation.x;
        q_tag_lv3.y= msg.pose.pose.orientation.y;
        q_tag_lv3.z= msg.pose.pose.orientation.z;
        q_tag_lv3.w= msg.pose.pose.orientation.w;
        if(current_aid_angle.x==0&&current_aid_angle.y==0&&current_aid_angle.z==0&&current_aid_angle.w==0)
        {
            current_aid_angle.x=0;
            current_aid_angle.y=0;
            current_aid_angle.z=0;
            current_aid_angle.w=1;
        }
        q_vehicle = quatMultiply(q_cam_to_vehicle, q_tag_lv3);
        //qpt->qtp,inverse it
        q_vehicle.x = -q_vehicle.x;
        q_vehicle.y = -q_vehicle.y;
        q_vehicle.z = -q_vehicle.z;

        q_cam_to_vehicle_multi = quatMultiply(q_vehicle, current_aid_angle);
        //qtg
        q_cam_to_vehicle_inverse.x = q_cam_to_vehicle_multi.x;
        q_cam_to_vehicle_inverse.y = q_cam_to_vehicle_multi.y;
        q_cam_to_vehicle_inverse.z = q_cam_to_vehicle_multi.z;
        q_cam_to_vehicle_inverse.w = q_cam_to_vehicle_multi.w;
        //get qpg  然后再转rotation
        Eigen::Matrix3d plane_odom_rotation= Quaternion2RotationMatrix(current_aid_angle.x, current_aid_angle.y, current_aid_angle.z, current_aid_angle.w);
        Eigen::Matrix3d qgt_rotation= Quaternion2RotationMatrix(q_cam_to_vehicle_inverse.x, q_cam_to_vehicle_inverse.y, q_cam_to_vehicle_inverse.z, q_cam_to_vehicle_inverse.w);

        geometry_msgs::Vector3 rpgXcameraoffset =  R_multi_P(plane_odom_rotation, camera_offset_x, camera_offset_y, camera_offset_z);
        // tag_point_lv3.x set offset 0.2
        geometry_msgs::Vector3 rgtXpct =  R_multi_P(qgt_rotation, tag_point_lv3.x-0.2, tag_point_lv3.y, tag_point_lv3.z); 
        pose_tag_lv3.pose.position.x = current_aid_point.x + rpgXcameraoffset.x - rgtXpct.x;
        pose_tag_lv3.pose.position.y = current_aid_point.y + rpgXcameraoffset.y - rgtXpct.y;
        pose_tag_lv3.pose.position.z = current_aid_point.z + rpgXcameraoffset.z - rgtXpct.z;
        pose_tag_lv3.pose.orientation=q_cam_to_vehicle_multi;
        // low pass filter
        if(first_see_tag)
        {
            pose_tag_final_f = pose_tag_lv3;
            first_see_tag = false;
        }
        else{
            pose_tag_final_f = pose_tag_filter(pose_tag_lv3, pose_tag_final_f, 0.1);
        }

        geometry_msgs::Vector3 temp11 = toEulerAngle(pose_tag_lv3.pose.orientation);
        // ROS_INFO("tag position 3:   %f %f %f",pose_tag_lv3.pose.position.x,pose_tag_lv3.pose.position.y,pose_tag_lv3.pose.position.z);
        // ROS_INFO("tag angle 3:   %f %f %f",temp11.x*rad2deg,temp11.y*rad2deg,temp11.z*rad2deg);
        time_tag_update_lv3 = ros::Time::now().toSec();
        tag_point_flag_lv3 = true;
        tag_point_any = true;
    }

}

void local_odom_callback(const nav_msgs::Odometry & current_info)
{
    time_current_update = ros::Time::now().toSec();
    current_aid_point.x= current_info.pose.pose.position.x;
    current_aid_point.y= current_info.pose.pose.position.y;
    current_aid_point.z= current_info.pose.pose.position.z;
    current_aid_angle.x= current_info.pose.pose.orientation.x;
    current_aid_angle.y= current_info.pose.pose.orientation.y;
    current_aid_angle.z= current_info.pose.pose.orientation.z;
    current_aid_angle.w= current_info.pose.pose.orientation.w;
    curr_aid_angle = toEulerAngle(current_aid_angle);
    // ROS_INFO("---------->curr angllllllllllle: %f", curr_aid_angle.z*rad2deg);
    current_aid_yaw_angle = curr_aid_angle.z;
    current_z_speed = current_info.twist.twist.linear.z;
}

void att_target_callback(const mavros_msgs::AttitudeTarget & att)
{
    geometry_msgs::Quaternion angle;
    angle.x = att.orientation.x;
    angle.y = att.orientation.y;
    angle.z = att.orientation.z;
    angle.w = att.orientation.w;
    current_imu_angle = toEulerAngle(angle);
}

void relheight_callback(const mavros_msgs::Altitude & rel_height)
{
    height_curr = rel_height.relative;
}

void keyboardCallback(const std_msgs::String key)
{
    key_receive = key;

}

void rcin_callback(const mavros_msgs::RCIn & rcvalue)
{
    channel5_value = rcvalue.channels[4];
    channel6_value = rcvalue.channels[5];
    channel7_value = rcvalue.channels[6];
    //channel 8 is the offboard press , be careful!
    channel8_value = rcvalue.channels[7];
}

void rcout_callback(const mavros_msgs::RCOut & rcvalue)
{
    //receive signal from FCU
    rc_out_signal_value = rcvalue.channels[15];
    // ROS_INFO("---->rc_out_signal_value: %d",rc_out_signal_value);
}

void mavlinkfrom_callback(const mavros_msgs::Mavlink &mav_msg)
{
    // ROS_INFO("---->receive msg id: %d", mav_msg.msgid);
    if(mav_msg.msgid == 112)
    {
        ROS_INFO("---->receive camera triggerr.............r");
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "off_board_test");
    ros::NodeHandle nh;
    nh.param<int>("tag_lv1",tag_size_lv1,0);
    nh.param<int>("tag_lv2",tag_size_lv2,0);
    nh.param<int>("tag_lv3",tag_size_lv3,0);
    nh.param<int>("vehicle_id",vehicle_id_,1);


    //test

    // q_cam_to_vehicle =tf:: createQuaternionMsgFromRollPitchYaw(180*deg2rad,89.9*deg2rad,0);
    // ROS_INFO("-------q_cam_to_vehicle: %f %f %f %f ", q_cam_to_vehicle.x, q_cam_to_vehicle.y, q_cam_to_vehicle.z, q_cam_to_vehicle.w);

    //define qcp
    q_cam_to_vehicle.x = 0.7071;
    q_cam_to_vehicle.y = 0.0;
    q_cam_to_vehicle.z  = -0.7071;
    q_cam_to_vehicle.w = 0.0;



    odometrysub     = nh.subscribe("vins_estimator/odometry",10,odometry_callback);
    circle_sub  =  nh.subscribe("/tracker/pos_image",10,circlepoint_callback);
    tagdect_sub = nh.subscribe("/tag_Odometry",10, tagdetect_callback);
    rcin_sub = nh.subscribe("/mavros/rc/in",10,rcin_callback);
    rcout_sub = nh.subscribe("/mavros/rc/out",10,rcout_callback);
    mavlinkfrom_sub = nh.subscribe("/mavlink/from",10,mavlinkfrom_callback);
    local_odom_sub = nh.subscribe("/mavros/local_position/odom",10,local_odom_callback);
    att_target_sub = nh.subscribe("/mavros/setpoint_raw/target_attitude",10,att_target_callback);
    relheight_sub = nh.subscribe("/mavros/altitude",10,relheight_callback);
    keyboard_sub = nh.subscribe("/keys",10,keyboardCallback);
    state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
    scan_tf_sub = nh.subscribe("/scan", 10, scan_tf_callback);
    gps_frommaster_sub = nh.subscribe("/gps/fromothermaster",10,gps_frommaster_callback);

    local_att_pub = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 10);
    rc_ov_pub = nh.advertise<mavros_msgs::OverrideRCIn>("/mavros/rc/override", 10);
    local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10);
    local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
    //自定义消息：发送是否利用apriltag着陆到flag
    ifland_pub = nh.advertise<std_msgs::Bool>("/thisvehicle/ifland",10);
    plot_msg_pub = nh.advertise<std_msgs::Float32MultiArray>("/plot_param3",10);

    arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    land_client = nh.serviceClient<mavros_msgs::CommandTOL>("mavros/cmd/land");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
    set_vechile_ned = nh.serviceClient<mavros_msgs::SetMavFrame>("mavros/setpoint_velocity/mav_frame");

    //get and set param
    get_param_client = nh.serviceClient<mavros_msgs::ParamGet>("mavros/param/get");
    set_param_client = nh.serviceClient<mavros_msgs::ParamSet>("mavros/param/set");

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(10);
 
    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        std::cout<<"----------------------->wait for FCU connection"<<std::endl;
        ros::spinOnce();
        rate.sleep();
    }

 
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    
    mavros_msgs::SetMode pos_set_mode;
    pos_set_mode.request.custom_mode = "POSITION";
 
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    mavros_msgs::CommandBool disarm_cmd;
    disarm_cmd.request.value = false;

    mavros_msgs::SetMavFrame setmavframe;
    setmavframe.request.mav_frame = 8;

    mavros_msgs::ParamGet get_nav_ft_dst;
    get_nav_ft_dst.request.param_id = "RC18_TRIM";

    mavros_msgs::ParamSet set_nav_ft_dst;
    set_nav_ft_dst.request.param_id = "RC18_TRIM";
    set_nav_ft_dst.request.value.real = 1500;
 
    ros::Time last_request = ros::Time::now();
    ros::Time last_request2 = ros::Time::now();
    ros::Time last_request3 = ros::Time::now();
    int time_wait = 0;

    // if(set_vechile_ned.call(setmavframe) && setmavframe.response.success)
    // {
    //     std::cout<<"------------>setmavframe success"<<std::endl;
    // }
    // else{
    //     std::cout<<"------------>setmavframe failed"<<std::endl;
    // }
 
    int cnt_temp = 0;
    bool receive_param_signal = false;
    while(ros::ok()){
        if(IFXUANTING)
        {
            if(HAVEPOS)
            {
                if(USETAG)
                {
                    //request param  from groundstation and then reset it
                    if((ros::Time::now() - last_request3 > ros::Duration(5.0))){
                        get_param_client.call(get_nav_ft_dst);
                        if(get_nav_ft_dst.response.value.real==1000){
                            ROS_INFO("get  the signal 1000 ....");
                            if(!receive_param_signal)
                            {
                                receive_param_signal = true;
                            }
                            //reset param to 1500
                            set_param_client.call(set_nav_ft_dst);

                        }
                        last_request3 = ros::Time::now();
                    }


                    //othervehicle_x.command    1->armed    2->disarmed &&land_flag=false  3->disarmed&&land_flag=true  
                    if(vehicle_id_ == 1)
                    {
                        // wait for signal to land 
                        if(channel7_value >1600 && !ifland_ && receive_param_signal) //(channel7_value >1800 && !ifland_ && receive_param_signal)
                        {
                            self_vehicle_down = true;
                        }
                        else{
                            self_vehicle_down = false;
                        }                        
                    }
                    // self_vehicle_down = true;

                    //takeoff and go to home position
                    if(self_vehicle_down)
                    {
                        //request to change state to offboard
                        if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
                            if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
                                ROS_INFO("Offboard enabled");
                            }
                            last_request = ros::Time::now();
                        } 
                        else {
                            //request to arm the vehicle
                            if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))){
                                if( arming_client.call(arm_cmd) && arm_cmd.response.success){
                                    ROS_INFO("Vehicle armed");
                                }
                                last_request = ros::Time::now();
                            }
                        }
                        if(height_takeoff_b)
                        {
                            //lock the takeoff height
                            height_takeoff = current_aid_point.z;
                            height_takeoff_b = false;
                        }
                        if((current_aid_point.z-height_takeoff)>2.9)
                        {
                            //flight height must be higher than 2.9
                            height_enough = true;
                        }

                        if(abs(current_aid_point.x)<0.3 && abs(current_aid_point.y)<0.3 && height_enough)
                        {
                            //如果到home点上方 直接跳过去home点的过程
                            up_home = true;
                        }

                        //go to the home position
                        // up_home = true;
                        if(!up_home)
                        {
                            //take off to 3m
                            if(!height_enough)
                            {                    
                                pose_send.pose.position.x = current_aid_point.x;
                                pose_send.pose.position.y = current_aid_point.y;
                                pose_send.pose.position.z = height_takeoff+3.0;
                                pose_send.pose.orientation.x = current_aid_angle.x;
                                pose_send.pose.orientation.y = current_aid_angle.y;
                                pose_send.pose.orientation.z = current_aid_angle.z;
                                pose_send.pose.orientation.w = current_aid_angle.w;
                                local_pos_pub.publish(pose_send);
                            }
                            else
                            {
                                pose_send.pose.position.x = 0;
                                pose_send.pose.position.y = 0;
                                pose_send.pose.position.z = current_aid_point.z;
                                pose_send.pose.orientation.x = current_aid_angle.x;
                                pose_send.pose.orientation.y = current_aid_angle.y;
                                pose_send.pose.orientation.z = current_aid_angle.z;
                                pose_send.pose.orientation.w = current_aid_angle.w;
                                local_pos_pub.publish(pose_send);
                            }
                            ROS_INFO("goto home position......");     
                            ros::spinOnce();
                            rate.sleep();
                            continue;
                        }
                    }
                    else
                    {
                        //print state message 
                        if(receive_param_signal)
                        {
                            ROS_INFO("receive signal but rc is locked...");
                        }
                        else if(ifland_)
                        {
                            ROS_INFO("already land ...");
                        }
                        else{
                            ROS_INFO("waiting for land signal ...");
                        }
                        
                        ros::spinOnce();
                        rate.sleep();
                        continue;
                    }

                    if(self_vehicle_down&&tag_point_any)
                    {
                        pose_send.pose.position.x = pose_tag_final_f.pose.position.x;
                        pose_send.pose.position.y = pose_tag_final_f.pose.position.y;

                        //judge height mode
                        if(current_aid_point.z-pose_tag_final_f.pose.position.z>2.5)
                        {
                            if(abs(pose_tag_final_f.pose.position.x-current_aid_point.x<0.2)&&abs(pose_tag_final_f.pose.position.y-current_aid_point.y<0.2))
                            {
                                pose_send.pose.position.z = current_aid_point.z - 0.3;
                            }
                            else{
                                pose_send.pose.position.z = current_aid_point.z;
                            }
                        }
                        else if(current_aid_point.z-pose_tag_final_f.pose.position.z>1.0)
                        {
                            if(abs(pose_tag_final_f.pose.position.x-current_aid_point.x<0.15)&&abs(pose_tag_final_f.pose.position.y-current_aid_point.y<0.15))
                            {
                                pose_send.pose.position.z = current_aid_point.z - 0.25;
                            }
                            else{
                                pose_send.pose.position.z = current_aid_point.z;
                            }
                        }
                        else if(current_aid_point.z-pose_tag_final_f.pose.position.z>0.5)
                        {
                            if(abs(pose_tag_final_f.pose.position.x-current_aid_point.x<0.1)&&abs(pose_tag_final_f.pose.position.y-current_aid_point.y<0.1))
                            {
                                pose_send.pose.position.z = current_aid_point.z - 0.2;
                            }
                            else{
                                pose_send.pose.position.z = current_aid_point.z;
                            }
                        }
                        else
                        {
                            //auto land if height is too low to land
                            ROS_INFO("Vehicle ready to land...");
                            mavros_msgs::CommandTOL srv_land;
                            srv_land.request.altitude = 0;
                            srv_land.request.latitude = 0;
                            srv_land.request.longitude = 0;
                            srv_land.request.yaw = 0;
                            if( current_state.mode == "OFFBOARD" && current_state.mode != "AUTO.LAND" && (ros::Time::now() - last_request2 > ros::Duration(5.0)) && abs(current_plane_point.x)<0.05&&abs(current_plane_point.y)<0.05)
                            {
                                if( land_client.call(srv_land) && srv_land.response.success){
                                    ifland_ = true;
                                    ROS_INFO("Vehicle landing...");
                                }
                                last_request2 = ros::Time::now();

                            }
                            ros::spinOnce();
                            rate.sleep();
                            continue;
                        }


                        pose_send.pose.orientation = pose_tag_final_f.pose.orientation;
                        //publish the position control message to mavros
                        local_pos_pub.publish(pose_send);
                        cnt_temp++;
                        if(cnt_temp>10)
                        {
                        ROS_INFO("tag position :   %f %f %f",pose_send.pose.position.x,pose_send.pose.position.y,pose_send.pose.position.z);
                        geometry_msgs::Vector3 temp_angle111;
                        temp_angle111 = toEulerAngle(pose_send.pose.orientation);
                        ROS_INFO("tag orientation :   %f ",temp_angle111.z*rad2deg);
                        cnt_temp = 0;
                        }
                    }




                    else //if(ros::Time::now().toSec() - time_tag_update_lv1 >= 0.2&&ros::Time::now().toSec() - time_tag_update_lv2 >= 0.2&&ros::Time::now().toSec() - time_tag_update_lv3 >= 0.2)
                    {
                        //如果检测不到tag，则原地悬停
                        pose_send.pose.position.x = current_aid_point.x;
                        pose_send.pose.position.y = current_aid_point.y;
                        pose_send.pose.position.z = current_aid_point.z;
                        pose_send.pose.orientation.x = current_aid_angle.x;
                        pose_send.pose.orientation.y = current_aid_angle.y;
                        pose_send.pose.orientation.z = current_aid_angle.z;
                        pose_send.pose.orientation.w = current_aid_angle.w;
                        local_pos_pub.publish(pose_send);
                        ROS_INFO(" no tag detected ............");
                        

                    }
                    //publish plot message
                    std_msgs::Float32MultiArray msg;
                    msg.data.push_back(current_aid_point.x);   
                    msg.data.push_back(current_aid_point.y);   
                    msg.data.push_back(current_aid_point.z);   //local position
                    msg.data.push_back(curr_aid_angle.x);   
                    msg.data.push_back(curr_aid_angle.y);  
                    msg.data.push_back(curr_aid_angle.z); //local angle
                    msg.data.push_back(pose_send.pose.position.x);
                    msg.data.push_back(pose_send.pose.position.y);
                    msg.data.push_back(pose_send.pose.position.z); //tag position
                    msg.data.push_back(tag_angle_lv1.x); //tag_angle_lv1
                    msg.data.push_back(tag_angle_lv2.x); //tag_angle_lv2
                    msg.data.push_back(tag_angle_lv3.x); //tag_angle_lv3

                    
                    plot_msg_pub.publish(msg); 


          
                }
                
            }
        }
        ros::spinOnce();
        rate.sleep();
    }
 
    return 0;
}



void position_pid_control_with_tag(geometry_msgs::Point current_set_point,geometry_msgs::Point current_local_point,float velocity_limit,float current_yaw, float dead_zone, uint8_t mode)
{
    float vx = 0.0, vy = 0.0, vxp = 0.0, vyp = 0.0, vxi = 0.0, vyi = 0.0, vxd = 0.0, vyd = 0.0;
    float yaw_rate = 0.0, yaw_rate_p = 0.0, yaw_rate_i = 0.0, yaw_rate_d = 0.0, vz = 0;
    float roll = 0.0, pitch = 0.0;
    //position control  mode & 0x01
    if (1) {
        //calculate velocity, P control
        geometry_msgs::Point error_pos;
        error_pos.x = current_set_point.x - current_local_point.x;
        error_pos.y = current_set_point.y - current_local_point.y;
        vxp = P_pos * error_pos.x;
        vyp = P_pos * error_pos.y;
        vxd = D_pos * (error_pos.x - error_pos_last.x);
        vyd = D_pos * (error_pos.y - error_pos_last.y);
        vxd = limit(vxd, D_VEL_LIMIT);
        vyd = limit(vyd, D_VEL_LIMIT);
        if (abs(error_pos.x) >= POS_I_DEAD_ZONE) {
            error_pos_integrated.x += error_pos.x;
        } else {
            error_pos_integrated.x = 0.0;
        }
        if (abs(error_pos.y) >= POS_I_DEAD_ZONE) {
            error_pos_integrated.y += error_pos.y;
        } else {
            error_pos_integrated.y = 0.0;
        }
        if (I_pos > 0.0001) {
            error_pos_integrated.x = limit((float) error_pos_integrated.x, I_VEL_LIMIT / I_pos);
            error_pos_integrated.y = limit((float) error_pos_integrated.y, I_VEL_LIMIT / I_pos);
        }
        vxi = I_pos * error_pos_integrated.x;
        vyi = I_pos * error_pos_integrated.y;
        vx = vxp + vxi + vxd;
        vy = vyp + vyi + vyd;

        float x_offset = current_set_point.x - current_local_point.x;
        float y_offset = current_set_point.y - current_local_point.y;
        float distance = sqrt(x_offset * x_offset + y_offset * y_offset);
        if (distance <= dead_zone) {
            dead_zone_flag = true;
            error_pos_integrated.x = 0.0;
            error_pos_integrated.y = 0.0;
            vx = 0;
            vy = 0;
        } else {
            dead_zone_flag = false;
        }

        //limit the speed
        vx = limit_velocity(vx, vy, velocity_limit).x;
        vy = limit_velocity(vx, vy, velocity_limit).y;
        //ROS_INFO("vx_exp vy_exp %f %f",vx,vy);

        error_pos_last = error_pos;
    }
    //yaw control  mode & 0x02
    if (1) {
        //use the flight controller yaw in default
        // float current_yaw = 0.0;
        // if(VIO)
        // {
        //     current_yaw = current_yaw_angle;
        // }
        // else if(GPS)
        // {
        //     current_yaw =current_aid_yaw_angle;
        // }
        //Drone turns at the smallest angle
        float error_yaw = 0.0;
        //use tag
        if(USETAG)
        {
            error_yaw = -current_yaw*rad2deg;
        }
        

        if (error_yaw < -180)
            error_yaw += 360;
        else if (error_yaw > 180)
            error_yaw -= 360;
        yaw_rate_p = P_yaw * error_yaw;
        yaw_rate_d = D_yaw * (error_yaw - error_yaw_last);
        error_yaw_integrated += error_yaw;
        if (I_yaw > 0.0001) {
            error_yaw_integrated = limit(error_yaw_integrated, YAW_I_LIMIT / I_yaw);
        }
        yaw_rate_i = I_yaw * error_yaw_integrated;
        if (abs(error_yaw) <= YAW_DEAD_ZONE) {
            yaw_rate_p = 0.0;
            yaw_rate_i = 0.0;
            yaw_rate_d = 0.0;
            error_yaw_integrated = 0.0;
        }
        yaw_rate = yaw_rate_p + yaw_rate_i + yaw_rate_d;
        if (abs(error_yaw) >= YAW_RATE_LIMIT) {
            yaw_rate = limit(yaw_rate, YAW_RATE_LIMIT);
        }
        error_yaw_last = error_yaw;
    }

    //height control  mode & 0x04
    if (1) {
        //control the height around 1.5m, the height is from flight controller
        float deltaz = current_local_point.z - current_set_point.z;
        if (fabs(deltaz) >= HEIGHT_DEAD_ZONE) {
            vz = P_z * (current_set_point.z - current_local_point.z);
        } else {
            vz = 0.0;
        }
    }
    velocity_ned.x = vx;
    velocity_ned.y = vy;
    velocity_ned.z = vz;
    velocity_expected.x = vx * cos(current_yaw) - vy * sin(current_yaw);
    velocity_expected.y = vy * cos(current_yaw) + vx * sin(current_yaw);
    velocity_expected.z = vz;
    attitude_expect.z = yaw_rate * deg2rad;

}

//position pid control without tag  
void position_pid_control(geometry_msgs::Point current_set_point,geometry_msgs::Point current_local_point,float velocity_limit,float target_yaw, float dead_zone, uint8_t mode)
{
    float vx = 0.0, vy = 0.0, vxp = 0.0, vyp = 0.0, vxi = 0.0, vyi = 0.0, vxd = 0.0, vyd = 0.0;
    float yaw_rate = 0.0, yaw_rate_p = 0.0, yaw_rate_i = 0.0, yaw_rate_d = 0.0, vz = 0;
    float roll = 0.0, pitch = 0.0;
    //position control  mode & 0x01
    if (1) {
        //calculate velocity, P control
        geometry_msgs::Point error_pos;
        error_pos.x = current_set_point.x - current_local_point.x;
        error_pos.y = current_set_point.y - current_local_point.y;
        vxp = P_pos * error_pos.x;
        vyp = P_pos * error_pos.y;
        vxd = D_pos * (error_pos.x - error_pos_last.x);
        vyd = D_pos * (error_pos.y - error_pos_last.y);
        vxd = limit(vxd, D_VEL_LIMIT);
        vyd = limit(vyd, D_VEL_LIMIT);
        /*if(abs(error_pos.x) < abs(error_pos_last.x) && (abs(error_pos.x) >= 1.5 * DEAD_ZONE))
        {
          vxd = 0.0;
        }
        if(abs(error_pos.y) < abs(error_pos_last.y) && (abs(error_pos.y) >= 1.5 * DEAD_ZONE))
        {
          vyd = 0.0;
        }*/
        if (abs(error_pos.x) >= POS_I_DEAD_ZONE) {
            error_pos_integrated.x += error_pos.x;
        } else {
            error_pos_integrated.x = 0.0;
        }
        if (abs(error_pos.y) >= POS_I_DEAD_ZONE) {
            error_pos_integrated.y += error_pos.y;
        } else {
            error_pos_integrated.y = 0.0;
        }
        if (I_pos > 0.0001) {
            error_pos_integrated.x = limit((float) error_pos_integrated.x, I_VEL_LIMIT / I_pos);
            error_pos_integrated.y = limit((float) error_pos_integrated.y, I_VEL_LIMIT / I_pos);
        }
        vxi = I_pos * error_pos_integrated.x;
        vyi = I_pos * error_pos_integrated.y;
        vx = vxp + vxi + vxd;
        vy = vyp + vyi + vyd;

        float x_offset = current_set_point.x - current_local_point.x;
        float y_offset = current_set_point.y - current_local_point.y;
        float distance = sqrt(x_offset * x_offset + y_offset * y_offset);
        if (distance <= dead_zone) {
            dead_zone_flag = true;
            error_pos_integrated.x = 0.0;
            error_pos_integrated.y = 0.0;
            vx = 0;
            vy = 0;
        } else {
            dead_zone_flag = false;
        }

        //limit the speed
        vx = limit_velocity(vx, vy, velocity_limit).x;
        vy = limit_velocity(vx, vy, velocity_limit).y;
        //ROS_INFO("vx_exp vy_exp %f %f",vx,vy);

        error_pos_last = error_pos;
    }
    //yaw control  mode & 0x02
    if (1) {
        //use the flight controller yaw in default
        
        float current_yaw = 0.0;
        if(VIO)
        {
            current_yaw = current_yaw_angle;
        }
        else if(GPS)
        {
            current_yaw =current_aid_yaw_angle;
        }
        //Drone turns at the smallest angle
        float error_yaw = (target_yaw - current_yaw) * rad2deg;
        if (error_yaw < -180)
            error_yaw += 360;
        else if (error_yaw > 180)
            error_yaw -= 360;
        yaw_rate_p = P_yaw * error_yaw;
        yaw_rate_d = D_yaw * (error_yaw - error_yaw_last);
        error_yaw_integrated += error_yaw;
        if (I_yaw > 0.0001) {
            error_yaw_integrated = limit(error_yaw_integrated, YAW_I_LIMIT / I_yaw);
        }
        yaw_rate_i = I_yaw * error_yaw_integrated;
        if (abs(error_yaw) <= YAW_DEAD_ZONE) {
            yaw_rate_p = 0.0;
            yaw_rate_i = 0.0;
            yaw_rate_d = 0.0;
            error_yaw_integrated = 0.0;
        }
        yaw_rate = yaw_rate_p + yaw_rate_i + yaw_rate_d;
        if (abs(error_yaw) >= YAW_RATE_LIMIT) {
            yaw_rate = limit(yaw_rate, YAW_RATE_LIMIT);
        } 
        // else {
        //     yaw_rate = limit(yaw_rate, (float) (YAW_RATE_LIMIT * 0.4));
        // }
        error_yaw_last = error_yaw;
    }

    //height control  mode & 0x04
    if (1) {
        //control the height around 1.5m, the height is from flight controller
        float deltaz = current_local_point.z - current_set_point.z;
        if (fabs(deltaz) >= HEIGHT_DEAD_ZONE) {
            vz = P_z * (current_set_point.z - current_local_point.z);
        } else {
            vz = 0.0;
        }
    }

 
    velocity_expected.x = vx * cos(current_aid_yaw_angle) + vy * sin(current_aid_yaw_angle);
    velocity_expected.y = vy * cos(current_aid_yaw_angle) - vx * sin(current_aid_yaw_angle);
    velocity_expected.z = vz;
    attitude_expect.z = yaw_rate * deg2rad;
}

void attitude_pid_control(geometry_msgs::Point current_set_point,geometry_msgs::Point current_local_point,float velocity_limit,float target_yaw, float dead_zone, uint8_t mode)
{
    float vx = 0.0, vy = 0.0, vxp = 0.0, vyp = 0.0, vxi = 0.0, vyi = 0.0, vxd = 0.0, vyd = 0.0;
    float yaw_rate = 0.0, yaw_rate_p = 0.0, yaw_rate_i = 0.0, yaw_rate_d = 0.0, vz = 0;
    float roll = 0.0;
    float pitch = 0.0;
    geometry_msgs::Point error_pos;
    error_pos.x = current_set_point.x - current_local_point.x;
    error_pos.y = current_set_point.y - current_local_point.y;
    vxp = P_pos * error_pos.x;
    vyp = P_pos * error_pos.y;
    vxd = D_pos * (error_pos.x - error_pos_last.x);
    vyd = D_pos * (error_pos.y - error_pos_last.y);
    vxd = limit(vxd, D_VEL_LIMIT);
    vyd = limit(vyd, D_VEL_LIMIT);
    if (abs(error_pos.x) >= POS_I_DEAD_ZONE) {
        error_pos_integrated.x += error_pos.x;
    } else {
        error_pos_integrated.x = 0.0;
    }
    if (abs(error_pos.y) >= POS_I_DEAD_ZONE) {
        error_pos_integrated.y += error_pos.y;
    } else {
        error_pos_integrated.y = 0.0;
    }
    if (I_pos > 0.0001) {
        error_pos_integrated.x = limit((float) error_pos_integrated.x, I_VEL_LIMIT / I_pos);
        error_pos_integrated.y = limit((float) error_pos_integrated.y, I_VEL_LIMIT / I_pos);
    }
    vxi = I_pos * error_pos_integrated.x;
    vyi = I_pos * error_pos_integrated.y;
    vx = vxp + vxi + vxd;
    vy = vyp + vyi + vyd;

    //vx vy -> roll pitch
    pitch = vx*k_pitch;
    roll = -vy*k_roll; 
    ROS_INFO("send to vechile: %f %f %f  ", pitch, roll, current_imu_angle.z);


    quaternion_att_control=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,current_imu_angle.z);

}

geometry_msgs::Vector3 toEulerAngle(geometry_msgs::Quaternion quat)
{
    geometry_msgs::Vector3 ans;
    
    tf::Matrix3x3 R_FLU2ENU(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
    R_FLU2ENU.getRPY(ans.x, ans.y, ans.z);


    return ans;
}


geometry_msgs::Vector3 toEulerAngle_tag(geometry_msgs::Quaternion quat)
{
    geometry_msgs::Vector3 ans;
    ans.x = atan2(2.0f * (quat.y*quat.z + quat.w*quat.x), quat.w*quat.w - quat.x*quat.x - quat.y*quat.y + quat.z*quat.z);
    ans.y = asin(-2.0f * (quat.x*quat.z - quat.w*quat.y));
    ans.z = atan2(2.0f * (quat.x*quat.y + quat.w*quat.z), quat.w*quat.w + quat.x*quat.x - quat.y*quat.y - quat.z*quat.z);

    return ans;
}

geometry_msgs::Vector3 threeaxisrot(double r11, double r12, double r21, double r31, double r32){
    geometry_msgs::Vector3 res1;
    res1.x = atan2( r31, r32 );
    res1.y = asin ( r21 );
    res1.z = atan2( r11, r12 );
    return res1;
}

geometry_msgs::Vector3 quaternion2Euler(geometry_msgs::Quaternion q)
{
    geometry_msgs::Vector3 res;
    res = threeaxisrot( 2*(q.x*q.y + q.w*q.z),
                        q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
                        -2*(q.x*q.z - q.w*q.y),
                        2*(q.y*q.z + q.w*q.x),
                        q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z
                    );
    return res;
    
}

geometry_msgs::Point rotation_hehe(geometry_msgs::Vector3 angle)
{
    geometry_msgs::Point delta_distance;
    float Rbe[3][3] = {0};
    angle.z -= yaw_init;
    Rbe[0][0] = cos(angle.y) * cos(angle.z);
    Rbe[0][1] = cos(angle.z) * sin(angle.y) * sin(angle.x) - sin(angle.z) * cos(angle.x);
    Rbe[0][2] = cos(angle.z) * sin(angle.y) * cos(angle.x) + sin(angle.z) * sin(angle.x);
    Rbe[1][0] = cos(angle.y) * sin(angle.z);
    Rbe[1][1] = sin(angle.z) * sin(angle.y) * sin(angle.x) + cos(angle.z) * cos(angle.x);
    Rbe[1][2] = sin(angle.z) * sin(angle.y) * cos(angle.x) - cos(angle.z) * sin(angle.x);
    Rbe[2][0] = -sin(angle.y);
    Rbe[2][1] = sin(angle.x) * cos(angle.y);
    Rbe[2][2] = cos(angle.x) * cos(angle.y);
    delta_distance.x = Rbe[0][0] * DELTA_X + Rbe[0][1] * DELTA_Y + Rbe[0][2] * DELTA_Z;
    delta_distance.y = Rbe[1][0] * DELTA_X + Rbe[1][1] * DELTA_Y + Rbe[1][2] * DELTA_Z;
    delta_distance.z = Rbe[2][0] * DELTA_X + Rbe[2][1] * DELTA_Y + Rbe[2][2] * DELTA_Z;
    return delta_distance;
}


geometry_msgs::Point limit_velocity(float vx, float vy,float maximum)
{
	geometry_msgs::Point vel;
	float velocity = sqrt(vx * vx + vy * vy);
	if(maximum <= 0)
	{
		vel.x = 0;
		vel.y = 0;
	}
	if(velocity <= maximum)
	{
		vel.x = vx;
		vel.y = vy;
	}
	//if velocity is bigger than maximum, then limit the vx and vy, and keep the direction meanwhile.
	else
	{
		//the velocity must not be zero when in this step
		vel.x = vx / velocity * maximum;
		vel.y = vy / velocity * maximum;
	}
	return vel;
}

geometry_msgs::Quaternion quatMultiply(geometry_msgs::Quaternion q2, geometry_msgs::Quaternion q1) {
    geometry_msgs::Quaternion qr;
    // qr.w = q2.w*q1.w - q2.x*q1.x - q2.y*q1.y - q2.z*q1.z;
    // qr.x = q2.w*q1.x + q2.x*q1.w - q2.y*q1.z + q2.z*q1.y;
    // qr.y = q2.w*q1.y + q2.x*q1.z + q2.y*q1.w - q2.z*q1.x;
    // qr.z = q2.w*q1.z - q2.x*q1.y + q2.y*q1.x + q2.z*q1.w;

    qr.w = q1.w*q2.w-q1.x*q2.x-q1.y*q2.y-q1.z*q2.z;
    qr.x = q1.w*q2.x+q1.x*q2.w+q1.y*q2.z-q1.z*q2.y;
    qr.y = q1.w*q2.y-q1.x*q2.z+q1.y*q2.w+q1.z*q2.x;
    qr.z = q1.w*q2.z+q1.x*q2.y-q1.y*q2.x+q1.z*q2.w;

    return qr;
}

Eigen::Matrix3d Quaternion2RotationMatrix(const double x,const double y,const double z,const double w)  
{  
    Eigen::Quaterniond q;  
    q.x() = x;  
    q.y() = y;  
    q.z() = z;  
    q.w() = w;  
  
    Eigen::Matrix3d R = q.normalized().toRotationMatrix();   
    return R;  
} 


geometry_msgs::Vector3 R_multi_P(Eigen::Matrix3d R, double x, double y, double z)
{
    geometry_msgs::Vector3 output;
    output.x = R(0,0)*x+R(0,1)*y+R(0,2)*z;
    output.y = R(1,0)*x+R(1,1)*y+R(1,2)*z;
    output.z = R(2,0)*x+R(2,1)*y+R(2,2)*z;
    return output;

}

//低通滤波器
geometry_msgs::PoseStamped pose_tag_filter(geometry_msgs::PoseStamped pose, geometry_msgs::PoseStamped pose_f, float alpha_filter)
{
    geometry_msgs::PoseStamped pose_out;
    pose_out.pose.position.x = alpha_filter*pose.pose.position.x + (1-alpha_filter)*pose_f.pose.position.x;
    pose_out.pose.position.y = alpha_filter*pose.pose.position.y + (1-alpha_filter)*pose_f.pose.position.y;
    pose_out.pose.position.z = alpha_filter*pose.pose.position.z + (1-alpha_filter)*pose_f.pose.position.z;


    double judge = pose.pose.orientation.x*pose_f.pose.orientation.x+pose.pose.orientation.y*pose_f.pose.orientation.y+pose.pose.orientation.z*pose_f.pose.orientation.z+pose.pose.orientation.w*pose_f.pose.orientation.w;
    if(judge>=0)
    {
        pose_out.pose.orientation.x = alpha_filter*pose.pose.orientation.x + (1-alpha_filter)*pose_f.pose.orientation.x;
        pose_out.pose.orientation.y = alpha_filter*pose.pose.orientation.y + (1-alpha_filter)*pose_f.pose.orientation.y;
        pose_out.pose.orientation.z = alpha_filter*pose.pose.orientation.z + (1-alpha_filter)*pose_f.pose.orientation.z;
        pose_out.pose.orientation.w = alpha_filter*pose.pose.orientation.w + (1-alpha_filter)*pose_f.pose.orientation.w;
    }
    else{
        pose_out.pose.orientation.x = alpha_filter*pose.pose.orientation.x - (1-alpha_filter)*pose_f.pose.orientation.x;
        pose_out.pose.orientation.y = alpha_filter*pose.pose.orientation.y - (1-alpha_filter)*pose_f.pose.orientation.y;
        pose_out.pose.orientation.z = alpha_filter*pose.pose.orientation.z - (1-alpha_filter)*pose_f.pose.orientation.z;
        pose_out.pose.orientation.w = alpha_filter*pose.pose.orientation.w - (1-alpha_filter)*pose_f.pose.orientation.w; 
    }
    Eigen::Quaterniond q1;  
    q1.x() = pose_out.pose.orientation.x;  
    q1.y() = pose_out.pose.orientation.y;  
    q1.z() = pose_out.pose.orientation.z;  
    q1.w() = pose_out.pose.orientation.w;  
    Eigen::Quaterniond q2 = q1.normalized();

    pose_out.pose.orientation.x = q2.x();
    pose_out.pose.orientation.y = q2.y();
    pose_out.pose.orientation.z = q2.z();
    pose_out.pose.orientation.w = q2.w();




    // ROS_INFO("----->>pose_rpy: %f %f %f", pose_rpy.x, pose_rpy.y, pose_rpy.z);
    // ROS_INFO("----->>pose_f_rpy: %f %f %f", pose_f_rpy.x, pose_f_rpy.y, pose_f_rpy.z);

    return pose_out;
}